/* AK电机使用伺服模式速度位置环 扩展帧, YBT电机使用混合模式 标准帧*/

#ifndef MOTOR_CTRL_H
#define MOTOR_CTRL_H

#include <stdio.h>
#include <stdint.h>
#include <iostream>

#include "can_ctrl.h"

using namespace std;

#define MOTOR_NUM_MAX   3

/* 角度增大方向 */
typedef enum {
    DIR_CLOCKWISE = 1,        //顺时针
    DIR_COUNTER   = -1,       //逆时针
} Dir_Type;

typedef enum {
    MOTOR_TYPE_AK    = 0,
    MOTOR_TYPE_YBT   = 1,
} Motor_Type;

typedef enum {
    MOTOR_MODE_ENTER = 0,       //进入
    MOTOR_MODE_EXIT,            //退出(复位)
    MOTOR_MODE_ORIGIN,          //原点
    MOTOR_MODE_W,               //速度
    MOTOR_MODE_POS,             //位置
    MOTOR_MODE_T,               //力矩
    MOTOR_MODE_MIX ,            //混合
} Motor_Mode;

/* 电机结构 */
    
class Motor {
public:
    Motor(Can_Ctrl *p_can_ctrl, uint8_t id, Motor_Type type) : p_can_ctrl(p_can_ctrl), id(id), type(type)
        {std::cout<<"Motor"<<std::endl;}
    virtual ~Motor() = 0;                       //纯虚析构，抽象类
    int32_t init();                             //初始化
    int32_t deinit();                   

    virtual int32_t set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len) = 0;     //设置帧
    virtual int32_t set_origin(uint8_t mode) = 0;         //设置零点                         
    virtual int32_t set_motor_pos(double pos) = 0;        //角度
    virtual int32_t set_motor_w(double w) = 0;            //角速度
    virtual int32_t set_motor_t(double t) = 0;            //前馈力矩
    virtual int32_t set_motor_acc(double acc) = 0;        //加速度
    virtual int32_t set_motor_kp(double kp) = 0;          //位置刚度
    virtual int32_t set_motor_kd(double kd) = 0;          //速度刚度
    virtual uint8_t get_motor_mode_map(Motor_Mode mode) = 0;   //模式映射
    virtual void set_cur_pos(uint8_t data[]) = 0;               //设置当前位置
    double get_motor_acceptable_err() {return accept_err;}      //容许误差
    double get_motor_each_max() {return each_angle_max;}        //每次最大旋转角度
    double get_motor_cmd_freq() {return cmd_freq;}              //每次最大旋转角度
    double get_cur_pos() {return cur_pos;}
    void set_motor_none();                                   //参数清零
    uint8_t get_motor_id();
    Motor_Type get_motor_type();
    Dir_Type get_motor_dir() {return this->dir;}

    Can_Ctrl *p_can_ctrl = nullptr;

protected:
    uint32_t flag_mix = 0; //混合控制标志
    Dir_Type dir = DIR_CLOCKWISE;
    double pos;
    double w;
    double t;
    double kp;
    double acc;
    double kd;

    double cur_pos      = 0.0f;        //当前角度
    double accept_err   = 1.0f;        //容许误差(终值与期望的差)
    double each_angle_max = 10.0f;     //每次旋转最大角(保护作用)
    double cmd_freq     = 0.01f;        //命令间隔

private:
    uint8_t id;
    Motor_Type type;
};

class Motor_AK : public Motor {
public:
    Motor_AK(Can_Ctrl *p_can_ctrl, uint8_t id, Motor_Type type) : Motor(p_can_ctrl, id, type)
    {   
        this->accept_err        = 0.2f;
        this->each_angle_max    = 5.0f;
        this->cmd_freq          = 0.1f;
        this->dir               = DIR_CLOCKWISE;
        std::cout<<"Motor_AK"<<std::endl;
    }
    ~Motor_AK(){std::cout<<"~Motor_AK"<<std::endl;}
    int32_t set_motor_dataframe(uint8_t data[], Motor_Mode mode, double value[], uint8_t len) override;                  
    int32_t set_origin(uint8_t mode) override;                              
    int32_t set_motor_pos(double pos) override;  
    int32_t set_motor_w(double w) override;
    int32_t set_motor_t(double t) override; 
    int32_t set_motor_acc(double acc) override;
    int32_t set_motor_kp(double kp) override; 
    int32_t set_motor_kd(double kd) override; 
    uint8_t get_motor_mode_map(Motor_Mode mode) override;
    void set_cur_pos(uint8_t data[]) override;
protected:

private:
};

class Motor_YBT : public Motor {
public:
    Motor_YBT(Can_Ctrl *p_can_ctrl, uint8_t id, Motor_Type type) : Motor(p_can_ctrl, id, type)
    {
        this->accept_err        = 0.5f;                //YBT的精度差一点
        this->each_angle_max    = 5.0f;
        this->cmd_freq          = 0.01f;              //YBT最快500Hz
        this->dir               = DIR_CLOCKWISE;                
        std::cout<<"Motor_YBT"<<std::endl;
    }
    ~Motor_YBT(){std::cout<<"~Motor_YBT"<<std::endl;}
    int32_t set_motor_dataframe(uint8_t data[], Motor_Mode mode, double value[], uint8_t len) override;                    
    int32_t set_origin(uint8_t mode) override;                                       
    int32_t set_motor_pos(double pos) override;  
    int32_t set_motor_w(double w) override;
    int32_t set_motor_t(double t) override; 
    int32_t set_motor_acc(double acc) override;
    int32_t set_motor_kp(double kp) override; 
    int32_t set_motor_kd(double kd) override;
    uint8_t get_motor_mode_map(Motor_Mode mode) {return 0;} 
    void set_cur_pos(uint8_t data[]) override;
protected:

private:
};


/* 电机控制体 */
class Motor_Ctrl {
    
public:
    Motor_Ctrl(Can_Ctrl *p_can_ctrl, uint8_t num,  uint8_t id_array[], Motor_Type type_array[]);   //id_array元素务必不重复
    ~Motor_Ctrl();

    int motor_ctrl_init();
    int motor_ctrl_deinit();
    int motor_func(uint8_t num, Motor_Mode mode, double value[], uint8_t len);
    uint8_t get_motor_num(){return num;}
    Motor* get_motor_item(uint8_t num){return p_motor[num];}
protected:
    
private:
    Can_Ctrl *p_can_ctrl = nullptr;
    Motor *p_motor[MOTOR_NUM_MAX] = {nullptr};      //电机
    uint8_t num;

};

#endif